%模型固有属性值
l1=0.4;l2=1;l3=0.7;ad=1.2;rc1=0.2;
        rc2=0.5;
        rc3=0.35;
        m1=1.2;
        m2=3;
        m3=2.2;
        rp2=0.75;
        fx2=5;
        mm1=25;
        mm3=5;
        g=9.8;
        thea1=0;
        thea2=0.76879;
        thea3=1.68713;%起始角度 
%书本上公式结果
disp('书本公式结果')
me=mm1+fx2*l1*sin(thea1)-m1*g*rc1*cos(thea1)+(fx2*rp2*sin(thea1)-m1*g*l1*cos(thea1)-m2*g*rc2*cos(thea2))*...
(l1*sin(thea1-thea3)/(l2*sin(thea3-thea2)))-(m3*g*rc3*cos(thea3)+mm3)...
        *(l1*sin(thea1-thea2)/(l3*sin(thea3-thea2)))
%质心雅克比矩阵
A=[-rc1*sin(thea1)+rc1*cos(thea1)*i,0,0;-l1*sin(thea1)+l1*cos(thea1)*i,-rc2*sin(thea2)+rc2*cos(thea2)*i,0;0,0,-rc3*sin(thea3)+rc3*cos(thea3)*i];
%受力雅克比矩阵
B=[-l1*sin(thea1)+l1*cos(thea1)*i,-rp2*sin(thea2)+rp2*cos(thea2)*i,0];
%约束矩阵
D=[1;
   (l1*sin(thea1-thea3))/(l2*sin(thea3-thea2));
     (l1*sin(thea1-thea2))/(l3*sin(thea3-thea2))];
%重力矩阵
mg=[-m1*g*i,-m2*g*i,-m3*g*i];
%mg2=[-m1*g*i;-m2*g*i;-m3*g*i;];
%主动力矩阵
f=[-fx2];
%扭矩矩阵
mm=[mm1,0,-mm3];
%普通乘法结果
disp('矩阵普通乘法结果')
juzhenliju=mg*A*D+f*B*D+mm*D
%共轭转置乘法结果共轭转置行数列数对不上
%gongeliju=(mg2'*A)''*D
